#include "ms618_algbackse1tose3edge.h"

using namespace MeasurE618;
AlgBackSE1toSE3Edge::AlgBackSE1toSE3Edge(int i):AlgBase(i)
{

}

void AlgBackSE1toSE3Edge::operator()(cv::Mat &srcImg,
                                     int id,
                                     std::map<string, float> &measRes,
                                     int fixtureId)
{

//    qDebug() << " ";
    //LD部分边缘提取，返回点位   //SE1,SE2,SE2,SE3,SE3 \paras618\back\SE.xml 前五行；
//    int LDmeasure(cv::Mat srcimg, const vector<int> paras, vector<cv::Point2f>& respts);
    std::vector<cv::Point2f> resptVec;
    {
        int i=17;
        std::vector<int> tparas;
        for(auto j=0;j< m_dataAllVec[i].m_edgeParam.cols;j++)
        {
            tparas.push_back(m_dataAllVec[i].m_edgeParam.at<int>(i-17,j));
        }
        std::vector<cv::Rect> rangVec;
        TemplateMatch(srcImg,m_dataAllVec[i],rangVec);
        cv::Mat imgt=srcImg(rangVec[0]).clone();
        std::vector<cv::Point2f> contours;
        edgedetect obj;
        obj.LDmeasure(imgt,tparas,contours);
        ImageCoordinateTransformation(contours,rangVec);

        std::unique_lock<std::mutex> lock(*AlgBase::mtxptrVec[id]);
        while (!AlgBase::flagVec[id])
        {
            (*AlgBase::cvarptrVec[id]).wait(lock);
        }
        for(auto &elem:contours)
        {
            cv::Point2f ptsIn(static_cast<float>(elem.x),static_cast<float>(elem.y)),ptsOut;
//            CoordinateTransformation(datumBackVec[id],ptsIn,ptsOut);
            CoordinateTransformationRelateToDot(datumBackVec[id], ptsIn, ptsOut);
            resptVec.push_back(ptsOut);
        }
    }

    {
        int i=18,cnt=1;
        while(cnt<3)
        {
            std::vector<int> tparas;
            for(auto j=0;j< m_dataAllVec[i].m_edgeParam.cols;j++)
            {
                tparas.push_back(m_dataAllVec[i].m_edgeParam.at<int>(cnt,j));
            }
            std::vector<cv::Rect> rangVec;
            TemplateMatch(srcImg,m_dataAllVec[i],rangVec);
            cv::Mat imgt=srcImg(rangVec[0]).clone();
            std::vector<cv::Point2f> contours;
            edgedetect obj;
            obj.LDmeasure(imgt,tparas,contours);
            ImageCoordinateTransformation(contours,rangVec);

            std::unique_lock<std::mutex> lock(*AlgBase::mtxptrVec[id]);
            while (!AlgBase::flagVec[id])
            {
                (*AlgBase::cvarptrVec[id]).wait(lock);
            }
            for(auto &elem:contours)
            {
                cv::Point2f ptsIn(static_cast<float>(elem.x),static_cast<float>(elem.y)),ptsOut;
//                CoordinateTransformation(datumBackVec[id],ptsIn,ptsOut);
                CoordinateTransformationRelateToDot(datumBackVec[id], ptsIn, ptsOut);
                resptVec.push_back(ptsOut);
            }
            cnt++;
        }

    }
    {
        int i=19,cnt=3;
        while(cnt<5)
        {
            std::vector<int> tparas;
            for(auto j=0;j< m_dataAllVec[i].m_edgeParam.cols;j++)
            {
                tparas.push_back(m_dataAllVec[i].m_edgeParam.at<int>(cnt,j));
            }
            std::vector<cv::Rect> rangVec;
            TemplateMatch(srcImg,m_dataAllVec[i],rangVec);
            cv::Mat imgt=srcImg(rangVec[0]).clone();
            std::vector<cv::Point2f> contours;
            edgedetect obj;
            obj.LDmeasure(imgt,tparas,contours);
            ImageCoordinateTransformation(contours,rangVec);

#ifdef MEASUR_E618_DRAW_IMG
        static int se3Index = 0;
        std::string imgName = ("se3_" + to_string(se3Index));
        imgName += ".jpg";
        se3Index++;
        cv::Mat se3Mat = srcImg.clone();
        for(int i = 0; i < contours.size(); ++i) {
            cv::circle(se3Mat, contours[i], 2, 160, -1);
            qDebug() << "se3_x: " << contours[i].x
                     << "se3_y: " << contours[i].y;
        }
        cv::imwrite(imgName, se3Mat);
#endif
            std::unique_lock<std::mutex> lock(*AlgBase::mtxptrVec[id]);
            while (!AlgBase::flagVec[id])
            {
                (*AlgBase::cvarptrVec[id]).wait(lock);
            }
            for(auto &elem:contours)
            {
                cv::Point2f ptsIn(static_cast<float>(elem.x),static_cast<float>(elem.y)),ptsOut;
//                CoordinateTransformation(datumBackVec[id],ptsIn,ptsOut);
                CoordinateTransformationRelateToDot(datumBackVec[id], ptsIn, ptsOut);
                resptVec.push_back(ptsOut);
            }
            cnt++;
        }

    }

    if(resptVec.size()!=13){
        cout<<"image error!"<<endl;
        return;
    }

    std::string prefix = m_xmlFileNamePre + to_string(fixtureId) + "_";
    float fai14=(resptVec[0].y+resptVec[1].y+resptVec[2].y)/3.0;
    measRes.insert(std::make_pair(string("FAI14-SE1-Y-distance_pixel"),fai14));
    float fai14y = CalcMeasureDistance(m_phypixpara[prefix + std::string("FAI14-SE1-Y-distance")], fai14);
    measRes.insert(std::make_pair(string("FAI14-SE1-Y-distance_real"), fai14y));

    float fai9=(resptVec[3].x+resptVec[4].x)/2.0;
    measRes.insert(std::make_pair(string("FAI9-SE2-X-distance_pixel"), -fai9));
    float fai9x = CalcMeasureDistance(m_phypixpara[prefix + std::string("FAI9-SE2-X-distance")], -fai9);
    measRes.insert(std::make_pair(string("FAI9-SE2-X-distance_real"), fai9x));

    float fai7=(resptVec[5].x+resptVec[6].x+resptVec[7].x)/3.0;
    measRes.insert(std::make_pair(string("FAI7-SE2-X-distance_pixel"), -fai7));
    float fai7x = CalcMeasureDistance(m_phypixpara[prefix + std::string("FAI7-SE2-X-distance")], -fai7);
    measRes.insert(std::make_pair(string("FAI7-SE2-X-distance_real"), fai7x));

    float fai3=(resptVec[8].y+resptVec[9].y)/2.0;
    measRes.insert(std::make_pair(string("FAI3-SE3-Y-distance_pixel"),fai3));
    float fai3y = CalcMeasureDistance(m_phypixpara[prefix + std::string("FAI3-SE3-Y-distance")], fai3);
    measRes.insert(std::make_pair(string("FAI3-SE3-Y-distance_real"), fai3y));

    float fai5=(resptVec[10].y+resptVec[11].y+resptVec[12].y)/3.0;
    measRes.insert(std::make_pair(string("FAI5-SE3-Y-distance_pixel"),fai5));
    float fai5y = CalcMeasureDistance(m_phypixpara[prefix + std::string("FAI5-SE3-Y-distance")], fai5);
    measRes.insert(std::make_pair(string("FAI5-SE3-Y-distance_real"), fai5y));
}
